#include "pid.h"
#include "main.h"


//ÊÊÓÃÓÚËÙ¶È»·PID
void PID_Updata(PID_t* P)
{
	
	P->Currerror=P->Target-P->Actual1;					   //µ±Ç°Îó²î=Ä¿±êÖµ-Êµ¼ÊÖµvvv的
	P->Currerror=(1-P->a)*P->Currerror+(P->a)*P->Preverror;//µÍÍ¨ÂË²¨½øÐÐÎó²î¼ÆËã
	P->Preverror=P->Currerror;							   //¸üÐÂÉÏ´ÎÎó²î

    if(P->Ki!=0)//Èç¹ûKi²»µÈÓÚ0,Ôò²»½øÐÐÎó²îµÄÀÛ»ý
    {
        P->ErrorInt+=P->Currerror;
    }
    else
        P->ErrorInt=0;
	
    int16_t Derivative=P->Currerror-P->Preverror;			//PDÏîµÄ¸üÐÂ
	
	 //Êä³öÏÞ·ù
	 if(P->Out>P->OUT_MAX)   
		 P->Out=P->OUT_MAX;             
	 if(P->Out<P->OUT_MIN)   
		 P->Out=P->OUT_MIN;
	 
	 //»ý·ÖÏÞ·ù
	 /*
	 if(P->ErrorInt>P->Error_MAX)
		P->ErrorInt=P->Error_MAX;
	 if(P->ErrotInt<P->Error_MIN)
		P->ErrotInt=P->Error_MIN;
	 */
    P->Out=(P->Kp)*P->Currerror+(P->Ki)*P->ErrorInt+(P->Kd)*Derivative;
}

//ÊÊÓÃÓÚ½Ç¶È»·PID
 float PID_Steering_Kp;    //×ªÏò»·Kp
 float PID_Steering_Kd;    //×ªÏò»·Kd
/**
 * @brief ×ªÏò»·(Î»ÖÃÊ½PD)
 * @param expect:ÆÚÍû×ªÏòµÄ½Ç¶È
 * @param angle:ÕæÊµµÄ½Ç¶È(Ò»°ãÊÇyaw)
 * @param gyro:ÕæÊµ½Ç¼ÓËÙ¶È(Ò»°ãÊÇgyro_z)
 * @return PWM
 * @author HZ12138
 * @date 2022-08-21 13:07:02
 */
int PID_Steering(int16_t expect, float angle, float gyro) 
{
	int PWM_OUT;
	float a=0.7;
	static float gyro_Last;
	gyro=(1-a)*gyro+a*gyro_Last;//µÍÍ¨ÂË²¨
	PWM_OUT=PID_Steering_Kp*(angle-expect)+PID_Steering_Kd*gyro;
	return PWM_OUT;
}









